/*
 * @file idle_server.cpp
 * @brief	no action from the user.
 *
 * @author	Peter Evers
 * @author	Michiel Hegemans
 * @date	Jan 2013
 */

#include "servers/idle_server.h"

/**
 * Constructor
 */
IdleAction::IdleAction(std::string name) : BaseServer(name),
	_as(nh_, name, boost::bind(&IdleAction::execute, this, _1), false)
{
	_as.start();
}

/**
 * Destructor
 */
IdleAction::~IdleAction() {
}

/**
 * Execute goal, do nothing :-).
 */
void IdleAction::execute(const rp_states::IdleGoalConstPtr &goal) {
	ros::Time stop_time = ros::Time::now() + goal->idle_duration;

	// open gripper
	comm::gripper msg;
	msg.state = GRIPPER_OPEN;
	publishGripper(msg);

	while (ros::Time::now() < stop_time && !(_as.isPreemptRequested()));

	if (_as.isPreemptRequested()) {
		ROS_INFO("Cancelled server before end time!");
		_as.setAborted();
		return;
	}

	_as.publishFeedback(_feedback);

	int pick = rand() % 3;

	switch (pick) {
	case 0: _result.action_selection = EMOTE_HAPPY; break;
	case 1: _result.action_selection = EMOTE_SAD; break;
	case 2: _result.action_selection = EMOTE_BORED; break;
	default: _result.action_selection = EMOTE_HAPPY; break;
	}

	ROS_INFO("Idle action, picked emotion: %i.", _result.action_selection);
	_as.setSucceeded(_result);
}

int main(int argc, char** argv) {
	ros::init(argc, argv, "idle");

	IdleAction idle(ros::this_node::getName());
	ROS_INFO("Idler online.");
	ros::spin();

	return 0;
}
